ndof = 5;

figpath = [
    '..' filesep ...
    '..' filesep ...
    'docs' filesep ...
    'report' filesep ...
    'figures' filesep];

figure(129);
set(gcf, ...
    'NumberTitle', 'off', ...
    'Name', 'MEEN 652: Control Inputs', ...
    'Position', [560 528 560 220], ...
    'PaperPosition', [2 3 4 1.5]);

q = data.integral_curves{end};
t = data.time{end} - min(data.time{end});

clf;
colors = [1 0 0; 0 1 0; 0 0 1; 0 1 1; 1 0 1];
lnames = cell(5, 1);

set(gca, 'ColorOrder', colors)
drawnow

for i = 1:5
    lnames{i} = ['$u_{' num2str(i) '}(t)$'];
end

lsiObj = LSysInfo();

T = cat(2, data.time{:});
u = nan(5, length(T));

k = 0;
for i = 1:length(data.time)
    t = data.time{i};
    x = data.integral_curves{i};
    
    for j = 1:length(t)
        k = k + 1;
        u(:, k) = lsiObj.getControl(...
            t(j), ...
            x(:, j));
    end
end



plot(T, u, 'LineWidth', 2);
legend(lnames, 'interpreter', 'latex');

xlabel('Time (sec)', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
ylabel('Torque (N $\cdot$ m))', ...
    'interpreter', 'latex', ...
    'FontSize', 12);
set(gca, ...
    'Position', [.12 .21 .84 .75], ...
    'FontSize', 12);
grid on
hgexport(gcf, [figpath 'sim_control.eps']);
